#!/usr/bin/env python3

# Applies torque to the steering wheel.

import rospy
import time

from pid import PID

from std_msgs.msg import Int16, UInt8, Float32
from geometry_msgs.msg import Point

last_target_time = 0
last_angle_time = 0
target = 0.0
angle = 0.0

def on_target(msg):
    global last_target_time, target
    last_target_time = time.time()
    target = msg.data

def on_angle(msg):
    global last_angle_time, angle
    last_angle_time = time.time()
    angle = msg.data

if __name__ == "__main__":
    rospy.init_node('steering_feedback_node')

    rospy.loginfo("init")

    param_topic_command = rospy.get_param("~topic_command", "command")
    param_topic_target = rospy.get_param("~topic_target", "target")
    param_topic_angle = rospy.get_param("~topic_angle", "/vehicle/steering_wheel_angle")
    param_max_power = rospy.get_param("~max_power", 700)

    pub_command = rospy.Publisher(param_topic_command, Int16, queue_size = 1)
    pub_pid = rospy.Publisher("pid", Point, queue_size = 1)
    pub_error = rospy.Publisher("error", Float32, queue_size = 1)

    sub_angle = rospy.Subscriber(param_topic_angle, Float32, on_angle)
    sub_target = rospy.Subscriber(param_topic_target, Float32, on_target)

    #feedback = PID(KP = 60.0, KI = 120.0, KD = 10.0, I_LIMIT = 10.0)
    feedback = PID(KP = 60.0, KI = 500.0, KD = 3.0, I_LIMIT = 0.5)
    #feedback = PID(KP = 100.0, KI = .0, KD = 0.0, I_LIMIT = 1.0)

    rospy.loginfo("spin")

    rate = rospy.Rate(60)

    while not rospy.is_shutdown():
        rate.sleep()
        t = time.time()

        if t - last_angle_time > 0.2:
            rospy.logwarn_throttle(4, "no steering angle data")
            continue

        if t - last_angle_time > 0.2:
            rospy.logwarn_throttle(4, "no steering target data")
            continue

        feedback.target = target
        power = -feedback.update(angle)

        msg = Int16()
        msg.data = int(max(min(power, param_max_power), -param_max_power))
        pub_command.publish(msg)

    rospy.loginfo("shutdown")
